import time
from param import *
from Arm_Lib import Arm_Device
from go_series import *
from serial_communication import serial_communicate


# 创建机械臂对象
Arm = Arm_Device()
time.sleep(.01)


def arm_standby(direction, claw=loose):
    # 待命位(左中右)  
     Arm.Arm_serial_servo_write6(direction + delta_yaw_servo_1, level_servo_2, level_servo_3, level_servo_4, 90, claw, 200)
     time.sleep(1)      
           
    
def arm_scout(direction):
    Arm.Arm_serial_servo_write6(direction + delta_yaw_servo_1, 90, 90, 3, 90, loose, 500)
    time.sleep(1)  
    
    
def arm_midway():
    # 中途防碰撞位
    Arm.Arm_serial_servo_write6(90, 90, 30, 30, 90, loose, 200)
    time.sleep(1)


def arm_grab(data_ik):
    # 居中待命
    Arm.Arm_serial_servo_write6(90 + delta_yaw_servo_1, level_servo_2, level_servo_3, level_servo_4, 90, loose, 200)
    time.sleep(1)
    # 中途防撞
    arm_midway()
    # 张开,前出,预备
    Arm.Arm_serial_servo_write6(data_ik[0], data_ik[1], data_ik[2], data_ik[3]-25, 90, loose, 200)
    time.sleep(1)
    # 张开,前出,到位
    Arm.Arm_serial_servo_write6(data_ik[0], data_ik[1], data_ik[2], data_ik[3], 90, loose, 200)
    time.sleep(1)
    # 抓紧
    Arm.Arm_serial_servo_write6(data_ik[0], data_ik[1], data_ik[2], data_ik[3], 90, tight, 200)
    time.sleep(1)
    # 抓紧，摘下
    Arm.Arm_serial_servo_write6(data_ik[0], data_ik[1], data_ik[2], data_ik[3]-15, 90, tight, 200)
    time.sleep(1)
    # 回中
    Arm.Arm_serial_servo_write6(90 + delta_yaw_servo_1, level_servo_2, level_servo_3, level_servo_4, 90, tight, 200)
    time.sleep(1)
    # 回中
    Arm.Arm_serial_servo_write6(90 + delta_yaw_servo_1, level_servo_2, level_servo_3, level_servo_4, 90, tight, 200)
    time.sleep(1)
    
    
    
def czq_zfz180():
    Arm.Arm_serial_servo_write6(90, 180, 0, 20, 90, tight, 200)  # 1
    time.sleep(.5)
    Arm.Arm_serial_servo_write6(180, 180, 0, 20, 90, tight, 200)  # 2
    time.sleep(.5)
    Arm.Arm_serial_servo_write6(180, 133, 24, 24, 90, tight, 200)  # 3
    time.sleep(.5)
    Arm.Arm_serial_servo_write6(180, 133, 24, 24, 90, 10, 200)  # 4
    time.sleep(.5)
    Arm.Arm_serial_servo_write6(180, 180, 0, -1, 90, 10, 200)  # 5
    time.sleep(.5)
    Arm.Arm_serial_servo_write6(90, 180, 0, -1, 90, 10, 200)  # 6
    time.sleep(.5)
    
def czq_yfz0():
    Arm.Arm_serial_servo_write6(90, 180, 0, 20, 90, tight, 200)  # 1
    time.sleep(.5)
    Arm.Arm_serial_servo_write6(0, 180, 0, 20, 90, tight, 200)  # 2
    time.sleep(.5)
    Arm.Arm_serial_servo_write6(0, 133, 24, 24, 90, tight, 200)  # 3
    time.sleep(.5)
    Arm.Arm_serial_servo_write6(0, 133, 24, 24, 90, 10, 200)  # 4
    time.sleep(.5)
    Arm.Arm_serial_servo_write6(0, 180, 0, -1, 90, 10, 200)  # 5
    time.sleep(.5)
    Arm.Arm_serial_servo_write6(90, 180, 0, -1, 90, 10, 200)  # 6
    time.sleep(.5)
    
def put_in_basket(direction):
    print("\nPut\n")
    # 放球            
    if direction == 180:
       serial_communicate("i")
       time.sleep(2)
       serial_communicate("l")
       return 1
         
    if direction == 0:
       serial_communicate("t")
       time.sleep(2)
       serial_communicate("r")
       return 1 

